/*
 * 自动控制时的全流程测试代码
 */
#include <AccelStepper.h>
#define Mode_DRIVER 1

#define forward_step 9 
#define forward_dir 8 
#define vertical_step 7 
#define vertical_dir 6   
#define rotate_step 5  
#define rotate_dir 4 
#define clutch_step 3 
#define clutch_dir 2 
#define vertical_init 0
#define clutch_init 2
#define rotate_init 3  
#define forward_init 1   
#define all_init 4  
void runStep(AccelStepper *step, long distanceToGoal, int runTime,int sub);
//清理串口缓存
void clearCache();
//自动工作时的控制函数
void automaticControl();
//以下是自动控制的八个工作流程
void task1(); // 回转到抓取处
void task2(); //下降爪子
void task3(); //收缩手爪
void task4(); //爪子上升+会转到放置处
void task5(); //下降爪子
void task6(); //松开手爪
void task7(); //上升爪子
void task8(byte times); //沿着导轨前进

int sub = 200; //细分
boolean num1 = 1,num2 = 1; //用以区分三种模式
volatile boolean stepIsInitial[4]; //判断旋转方向是否归零
boolean taskIsDown[7];  //在自动控制时，判断每一个任务是否完成
boolean work[7];  //在自动控制时，判断电机在每一个工作开始时是否初始化
byte times = 0; //在自动控制时判断完成了几轮循环工作
float t;
//定义四个电机
AccelStepper vertical(Mode_DRIVER, vertical_step, vertical_dir);
AccelStepper rotate(Mode_DRIVER, rotate_step, rotate_dir);
AccelStepper clutch(Mode_DRIVER, clutch_step, clutch_dir);
AccelStepper forward(Mode_DRIVER, forward_step, forward_dir);

void setup() {
  // put your setup code here, to run once:
  for (int j = 0; j < 4; j++)
  {
    stepIsInitial[j] = 1;
  }
  for (int j = 0; j < 7; j++)
  {
    taskIsDown[j] = 0;
    work[j] = 0;
  }
  Serial.begin(9600);
  Serial.println("请输入控制模式数字");
  Serial.println("1 ： 手动控制    0：自动控制");
  while (! Serial.available()){}
  num1 = Serial.parseInt();
  clearCache();
  if (num1 == 1)
  {
    Serial.println("请选择指令控制还是摇杆控制:");
    Serial.println("1 ： 指令控制    0：摇杆控制");
    while (!Serial.available()) {}
    num2 = Serial.parseInt();
    clearCache();
    Serial.print("当前选择为:");
    if (num2 == 1)
    {
      Serial.println("指令模式");
    }
    else {
      Serial.println("遥感模式");
    }
  }
  else
  {
    Serial.print("当前选择为:");
    Serial.println("自动控制模式");
  }
  Serial.println("工作模式选择完毕,现在开始工作");
  vertical.setCurrentPosition(0);
  forward.setCurrentPosition(0);
  rotate.setCurrentPosition(0);
  clutch.setCurrentPosition(0);
  t = millis();
}

void loop() {
  automaticControl();
  if(times == 5)
  {
    Serial.print((float)((millis()-t)/1000));
    Serial.print("s");
    while(1);
  }
}
void runStep(AccelStepper *step, long distanceToGoal, int runTime,int sub)
{
  long a, maxspeed;
  distanceToGoal = distanceToGoal * sub/360;
  step->moveTo(distanceToGoal);
  a = step->distanceToGo() / pow(runTime / 2, 2)+sub/4;
  maxspeed = a * runTime / 2+sub/4;
  step->setAcceleration(a);
  step->setMaxSpeed(maxspeed);
}
void clearCache() {
  while (Serial.read() > 0);
}
void automaticControl()
{
//  if (!stepIsInitial[0] || !stepIsInitial[1] || !stepIsInitial[2] || !stepIsInitial[3]) {
//    initialPosition(all_init);
//    if (stepIsInitial[0] && stepIsInitial[1] && stepIsInitial[2] && stepIsInitial[3])
//    {
//      Serial.println("已经准备就绪，输入s即可开始工作");
//      while (1)
//      {
//        if (Serial.available())
//        {
//          if (Serial.read() == 's')
//          {
//            clearCache();
//            break;
//          }
//          else {
//            clearCache();
//          }
//        }
//      }
//    }
//  }   
//  else
//  {
    if (!taskIsDown[0])
    {
      task1();
    }
    else {
      if (!taskIsDown[1]) {
        task2();
      }
      else {
        if (!taskIsDown[2]) {
          task3();
        }
        else {
          if (!taskIsDown[3]) {
            task4();
          }
          else {
            if (!taskIsDown[4]) {
              task5();
            }
            else {
              if (!taskIsDown[5]) {
                task6();
              }
              else {
                if (!taskIsDown[6]) {
                  task7();
                }
                else {
                  times++;
                  if (times < 5) {

                    for (int i = 0; i < 7; i++)
                    {
                      taskIsDown[i] = !taskIsDown[i];
                      work[i] = !work[i];
                    }
                      task8(times);
                  }       
                }
              }
            }
          }
        }
      }
    }
//  }
}
void task1()
{
  if (!work[0])
  {
    delay(100);

    runStep(&rotate, -180, 4, 6400);
    work[0] = !work[0];
  }
  else
  {
    rotate.run();
    forward.run();
    if (rotate.distanceToGo() == 0 && forward.distanceToGo() == 0)
    {
      taskIsDown[0] = !taskIsDown[0];
    }
  }
}
void task2()
{
  if (!work[1])
  {
    delay(100);
    runStep(&vertical, -900, 2, 400);
    work[1] = !work[1];
  }
  else
  {
    vertical.runToPosition();
    if (vertical.distanceToGo() == 0)
    {
      taskIsDown[1] = !taskIsDown[1];
    }
  }
}
void task3()
{
  if (!work[2])
  {
    delay(100);
    work[2] = !work[2];
    clutch.setMaxSpeed(120);
    clutch.setAcceleration(24);
    clutch.setSpeed(48);
    clutch.moveTo(0);
  }
  else
  {
//    clutch.runSpeedToPosition();
//    int N = analogRead(A0);
//    if (N < 512)
//    {
//      clutch.setCurrentPosition(0);
      
//    }
    taskIsDown[2] = !taskIsDown[2];
    delay(1000);
  }
}

void task4()
{
  if (!work[3])
  {
    delay(100);
    work[3] = !work[3];
    runStep(&rotate, 0, 4, 6400);
    runStep(&vertical, 0, 2, 400);
  }
  else {
    rotate.run();
    vertical.run();
    if (rotate.distanceToGo() == 0 && vertical.distanceToGo() == 0)
    {
      taskIsDown[3] = !taskIsDown[3];
    }
  }
}
void task5()
{
  if (!work[4])
  {
    delay(100);
    runStep(&vertical, -900, 2, 400);
    work[4] = !work[4];
  }
  else
  {
    vertical.runToPosition();
    if (vertical.distanceToGo() == 0)
    {
      taskIsDown[4] = !taskIsDown[4];
    }
  }
}
void task6()
{
  if (!work[5])
  {
    delay(100);
    runStep(&clutch, 240, 1, 24);
    work[5] = !work[5];
  }
  else
  {
//    clutch.runToPosition();
//    if (clutch.distanceToGo() == 0)
//    {
//      taskIsDown[5] = !taskIsDown[5];
//    }
    delay(1000);
    taskIsDown[5] = !taskIsDown[5];
  }
}

void task7()
{
  if (!work[6])
  {
    delay(100);
    runStep(&vertical, 0, 2, 400);
    work[6] = !work[6];
  }
  else
  {
    vertical.runToPosition();
    if (vertical.distanceToGo() == 0)
    {
      taskIsDown[6] = !taskIsDown[6];
    }
  }
}
void task8(byte times)
{
  runStep(&forward, -3600 * times, 4, 400);
}
